/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#include "PhysicalShape.h"
#include "Sphere.h"
#include "Cylinder.h"
#include "Capsule.h"
#include "Box.h"
#include "Plane.h"

template<>
dGeomID PhysicalShape<Sphere>::createGeom(dSpaceID colSpace) {
	Sphere* thiz = dynamic_cast<Sphere*>(this);
	dGeomID geom = dCreateSphere(colSpace, thiz->getRadius());
	return geom;
}

template<>
dGeomID PhysicalShape<Cylinder>::createGeom(dSpaceID colSpace) {
	Cylinder* thiz = dynamic_cast<Cylinder*>(this);
	dGeomID geom = dCreateCylinder(colSpace, thiz->getRadius(), thiz->getLength());
	return geom;
}

template<>
dGeomID PhysicalShape<Capsule>::createGeom(dSpaceID colSpace) {
	Capsule* thiz = dynamic_cast<Capsule*>(this);
	dGeomID geom = dCreateCapsule(colSpace, thiz->getRadius(), thiz->getLength());
	return geom;
}

template<>
dGeomID PhysicalShape<Box>::createGeom(dSpaceID colSpace) {
	Box* thiz = dynamic_cast<Box*>(this);
	dGeomID geom = dCreateBox(colSpace, thiz->getWidth(), thiz->getHeight(), thiz->getLength());
	return geom;
}

template<>
dGeomID PhysicalShape<Plane>::createGeom(dSpaceID colSpace) {
	Plane* thiz = dynamic_cast<Plane*>(this);
	dGeomID geom = dCreatePlane(colSpace, 0, 1, 0, 0);
	return geom;
}

template<>
void PhysicalShape<Sphere>::setMass(dMass* mass, float density) {
	Sphere* thiz = dynamic_cast<Sphere*>(this);
	dMassSetSphere(mass, density, thiz->getRadius());
}

template<>
void PhysicalShape<Cylinder>::setMass(dMass* mass, float density) {
	Cylinder* thiz = dynamic_cast<Cylinder*>(this);
	dMassSetCylinder(mass, density, 3, thiz->getRadius(), thiz->getLength());
}

template<>
void PhysicalShape<Capsule>::setMass(dMass* mass, float density) {
	Capsule* thiz = dynamic_cast<Capsule*>(this);
	dMassSetCapsule(mass, density, 3, thiz->getRadius(), thiz->getLength());
}

template<>
void PhysicalShape<Box>::setMass(dMass* mass, float density) {
	Box* thiz = dynamic_cast<Box*>(this);
	dMassSetBox(mass, density, thiz->getWidth(), thiz->getHeight(), thiz->getLength());
}

template<>
void PhysicalShape<Plane>::setMass(dMass* mass, float density) {
	Plane* thiz = dynamic_cast<Plane*>(this);
	dMassSetZero(mass);
}
